﻿
// 2019/4/22: 首个记录
// 2019/9/19: 增加USoundSensorProperty默认构造函数
// 2019/10/11: 修改pragma once和include
// 2020/1/14: 接口完整性
// 2020/11/12: 支持Spadas7
// 2022/11/2: 支持Spadas8.4
// 2023/10/11: 补充部分英文注释
// 2024/7/3: 基于新样本框架
// 2025/4/7: 实现插值和fromSample
// 2025/8/4: 改为继承SessionSample。改回单头文件

#ifndef USOUND_SAMPLE_V1_H
#define USOUND_SAMPLE_V1_H

#include "spadas.h"

namespace usound_sample_v1
{
	using namespace spadas;

	struct USoundSensorProperty
	{
		Double originX; // [m] X-coordinate of the n-th sensor's position / 传感器原点X坐标
		Double originY; // [m] Y-coordinate of the n-th sensor's position / 传感器原点Y坐标
		Double orientation; // [°] Central heading angle of the n-th sensor / 传感器中心轴朝向
		Double angleRangeAlpha; // [°] α angle range (generally around 100°, with sensor's origin as the center of the circle) / α角范围（一般在100°左右，以OriginX/Y为圆心）
		Double angleRangeBeta; // [°] β angle range (generally ranges 0~30°) / β角范围（一般在0~30°）
		Double distanceRangeAlpha; // [m] Detect range corresponding to α angle (shorter) / α角对应的探测距离（较短）
		Double distanceRangeBeta; // [m] Detect range corresponding to β angle (longer) / β角对应的探测距离（较长）

		USoundSensorProperty()
		{
			originX = 0;
			originY = 0;
			orientation = 0;
			angleRangeAlpha = 100;
			angleRangeBeta = 15;
			distanceRangeAlpha = 0.5;
			distanceRangeBeta = 2.5;
		}
	};

	struct USoundSensorData
	{
		Int id; // Sensor ID, starting from 1 / 传感器ID，从1开始
		USoundSensorProperty properties; // Sensor properties / 超声波传感器属性
		OptionalDouble distance; // [m] Detected barrier's distance, invalid indicates no barrier / 障碍物的距离，无障碍物则为空
	};

	struct USoundSampleData
	{
		Array<USoundSensorData> dataList; // Data of each sensor / 各个超声波传感器的数据

		USoundSampleData()
		{}
	};

	class USoundSample : public SessionSample
	{
	private:
		class USoundSampleVars : public SessionSampleVars
		{
		public:
			SPADAS_VARS(USoundSample, SessionSampleVars)
			USoundSampleData data;

			SessionGeneralSample toGeneralSample()
			{
				SessionGeneralSample sample;
				sample.timestamp() = timestamp;

				Array<GeneralElement> values(1 + 9 * data.dataList.size());
				sample.values() = values;

				values[0] = data.dataList.size();

				for (UInt i = 0; i < data.dataList.size(); i++)
				{
					USoundSensorData& src = data.dataList[i];
					values[1 + 9 * i] = src.id;
					values[2 + 9 * i] = src.distance;
					values[3 + 9 * i] = src.properties.originX;
					values[4 + 9 * i] = src.properties.originY;
					values[5 + 9 * i] = src.properties.orientation;
					values[6 + 9 * i] = src.properties.angleRangeAlpha;
					values[7 + 9 * i] = src.properties.angleRangeBeta;
					values[8 + 9 * i] = src.properties.distanceRangeAlpha;
					values[9 + 9 * i] = src.properties.distanceRangeBeta;
				}

				sample.significantCount() = values.size();
				return sample;
			}

			Bool fromSample(String protocol, SessionSample sample) override
			{
				SessionGeneralSample generalSample;
				if (!sample.is<SessionGeneralSample>(generalSample)) return FALSE;

				timestamp = generalSample.timestamp();
				auto values = generalSample.values();

				if (protocol == "usound-sample-v1")
				{
					if (values.size() < 1) return FALSE;
					if (!values[0].valid) return FALSE;

					UInt sensorCount = (UInt)values[0].value;
					if (values.size() != 1 + sensorCount * 9) return FALSE;

					data.dataList = Array<USoundSensorData>(sensorCount);
					for (UInt i = 0; i < sensorCount; i++)
					{
						USoundSensorData& sensor = data.dataList[i];
						UInt baseIndex = 1 + 9 * i;

						sensor.id = (Int)values[baseIndex].value;
						sensor.distance = values[baseIndex + 1];
						sensor.properties.originX = values[baseIndex + 2].value;
						sensor.properties.originY = values[baseIndex + 3].value;
						sensor.properties.orientation = values[baseIndex + 4].value;
						sensor.properties.angleRangeAlpha = values[baseIndex + 5].value;
						sensor.properties.angleRangeBeta = values[baseIndex + 6].value;
						sensor.properties.distanceRangeAlpha = values[baseIndex + 7].value;
						sensor.properties.distanceRangeBeta = values[baseIndex + 8].value;
					}

					return TRUE;
				}
				
				return FALSE;
			}
			
			Bool supportInterpolation() override
			{
				return TRUE;
			}

			SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
			{
				USoundSample us1, us2;
				if (!ss1.is<USoundSample>(us1) || !ss2.is<USoundSample>(us2)) return SessionSample();

				USoundSampleData& s1 = us1.var()->data;
				USoundSampleData& s2 = us2.var()->data;
				
				// 创建输出对象
				USoundSample output;
				output.timestamp() = timestamp;
				USoundSampleData& so = output.var()->data;
				
				// 合并传感器数据
				ArrayX<USoundSensorData> sensors;
				
				// 遍历第一个样本中的所有传感器数据
				for (auto sensor1 = s1.dataList.firstElem(); sensor1.valid(); ++sensor1)
				{
					// 在第二个样本中查找相同ID的传感器
					for (auto sensor2 = s2.dataList.firstElem(); sensor2.valid(); ++sensor2)
					{
						// 如果找到相同ID的传感器，则执行插值
						if (sensor1->id == sensor2->id)
						{
							USoundSensorData newSensor;
							newSensor.id = sensor1->id;
							
							// 对于Properties，直接使用第一个样本中的属性
							// 因为传感器的物理属性通常不会改变
							newSensor.properties.originX = sensor1->properties.originX;
							newSensor.properties.originY = sensor1->properties.originY;
							newSensor.properties.orientation = sensor1->properties.orientation;
							newSensor.properties.angleRangeAlpha = sensor1->properties.angleRangeAlpha;
							newSensor.properties.angleRangeBeta = sensor1->properties.angleRangeBeta;
							newSensor.properties.distanceRangeAlpha = sensor1->properties.distanceRangeAlpha;
							newSensor.properties.distanceRangeBeta = sensor1->properties.distanceRangeBeta;
							
							// 对于Distance，只有当两个样本中都有值时才进行插值
							// 否则设为无效值
							if (sensor1->distance.valid && sensor2->distance.valid)
							{
								newSensor.distance.valid = TRUE;
								newSensor.distance.value = sensor1->distance.value * w1 + sensor2->distance.value * w2;
							}
							else if (sensor1->distance.valid)
							{
								newSensor.distance = sensor1->distance;
							}
							else if (sensor2->distance.valid)
							{
								newSensor.distance = sensor2->distance;
							}
							
							sensors.append(newSensor);
							break;
						}
					}
				}
				
				so.dataList = sensors.toArray();
				return output.as<SessionSample>();
			}
		};

	public:
		SPADAS_CLASS("usound_sample_v1.USoundSample", USoundSampleVars)

		USoundSample() : SessionSample(new USoundSampleVars)
		{}
		USoundSampleData *operator ->()
		{
			return &var()->data;
		}

		static String getProtocolName()
		{
			return "usound-sample-v1";
		}
		SessionGeneralSample toGeneralSample()
		{
			return var()->toGeneralSample();
		}

		static Array<String> getProtocolNames()
		{
			return Array<String>::create(1, "usound-sample-v1");
		}
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample)
		{
			return fromSample(protocol, generalSample);
		}
	};
}

#endif